#include<ros/ros.h>  
#include<geometry_msgs/Twist.h>  
#include <sensor_msgs/Joy.h>  
#include<iostream>  
#include "std_msgs/UInt8MultiArray.h"
#include "std_msgs/UInt8.h"
using namespace std;  
  
class Teleop  
{  
public:  
    Teleop();  
  
private:  
    /* data */  
    void callback(const sensor_msgs::Joy::ConstPtr& Joy);  
    ros::NodeHandle n; //实例化节点  
    ros::Subscriber sub ;  
    ros::Publisher key ;  
    ros::Publisher pub ;  
    double vlinear,vangular;//我们控制乌龟的速度，是通过这两个变量调整  
    int axis_ang,axis_lin,add_velocity,dec_velocity;  //axes[]的键  
};  
  
Teleop::Teleop()  
{     
//下面按键的设置，一定要根据自己的实际情况来更改 
n.param<int>("axis_linear",axis_lin, 1); //默认axes[1]接收速度   
n.param<int>("axis_angular",axis_ang,2);//默认axes[2]接收角度
n.param<int>("button_add",add_velocity,0); //默认axes[1]接收速度   
n.param<int>("button_dec",dec_velocity,2);//默认axes[2]接收角度
n.param<double>("vel_linear",vlinear,1);//默认线速度1 m/s
n.param<double>("vel_angular",vangular,1);//默认角速度1 单位rad/s
pub = n.advertise<geometry_msgs::Twist>("/control_velocity_joy",1);//将速度发给乌龟
key = n.advertise<std_msgs::UInt8 >("/control_key",1);//将速度发给乌龟
sub = n.subscribe<sensor_msgs::Joy>("joy",10,&Teleop::callback,this); //订阅游戏手柄发来的数据
}
void Teleop::callback(const sensor_msgs::Joy::ConstPtr& Joy)  
{   
geometry_msgs::Twist v;
std_msgs::UInt8 m;
m.data = 0;

if (Joy->buttons[4] == 1)
{
    v.linear.x =Joy->axes[axis_lin]*vlinear *2; //将游戏手柄的数据乘以你想要的速度，然后发给乌龟
    v.angular.z =Joy->axes[axis_ang]*vangular * 2;  
    v.linear.y = Joy->buttons[add_velocity];
    v.angular.y = Joy->buttons[dec_velocity];
    ROS_INFO("linear:%.3lf angular:%.3lf",v.linear.x,v.angular.z);   
    pub.publish(v);  
    if (Joy->buttons[0] == 1)
        m.data |= 0x01;
    if (Joy->buttons[2] == 1)
        m.data |= 0x02;
    if (Joy->buttons[6] == 1)
        m.data |= 0x04;
    if (Joy->buttons[7] == 1)
        m.data |= 0x08;
    if (Joy->buttons[5] == 1)
        m.data |= 0x10;
    if (Joy->buttons[8] == 1)
        m.data |= 0x20;
    if (Joy->buttons[9] == 1)
        m.data |= 0x40;
    if (Joy->buttons[1] == 1)
        m.data |= 0x80;
    key.publish(m);
}
else
{
    v.linear.x = 0;
    v.linear.y = 0;
    v.angular.z = 0;
    v.angular.y = 0;
    pub.publish(v);
}
if(Joy->buttons[5] == 1 && Joy->buttons[5] == 1){
    v.linear.y = 0.123;
    pub.publish(v);
}

}  
int main(int argc,char** argv)  
{  
 ros::init(argc, argv, "logteleop");  
 Teleop telelog;
 ros::spin();
 return 0;  
}
